#include <rclcpp/rclcpp.hpp>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include "std_msgs/msg/string.hpp" 
#include <sstream>
#include <iostream>
#include<stdio.h>
#include "sensor_msgs/image_encodings.hpp"

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "rclcpp/rclcpp.hpp"

#include "../include/red/Params.h"
#include "../include/red/Person.h"

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"


class SubNode : public rclcpp::Node
{
public:
    SubNode(std::string name) : Node(name)
    {
        subscribe = this->create_subscription<sensor_msgs::msg::Image>("photo", 10, std::bind(&SubNode::doMsg, this, std::placeholders::_1));
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscribe;
    void doMsg(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        cv::Mat img;
        img=cv_bridge::toCvShare(msg,"bgr8")->image;
        
	    Params param;
        //图像预处理
        std::vector<cv::Mat> channels;
        cv::Mat grayimg,binBrightImg;
        //取蓝色通道    
        cv::split(img,channels);
        grayimg=(channels.at(2));
        //二值
        cv::threshold(grayimg, binBrightImg, param.deal_threshold, 255, cv::THRESH_BINARY);
        //膨胀
        
        //cv::Mat element = (cv::Mat_<double>(3,3)<<0,1,0,1,1,1,0,1,0);
        //cv::dilate(binBrightImg, binBrightImg, element);
        
        //找轮廓
        std::vector<std::vector<cv::Point> > Contours;
        std::vector<LightDescriptor> lights;
        cv::findContours(binBrightImg.clone(), Contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
        //挑选轮廓
        float contArea;cv::RotatedRect rect;
        for(int i=0;i<int(Contours.size());i++)
        {
            contArea=contourArea(Contours[i]);
            //面积约束
            if(contArea<param.choose_min_area || contArea>param.choose_max_area){continue;}
        
            rect=fitEllipse(Contours[i]);
            //长宽比例约束
            if(rect.size.height/rect.size.width<param.choose_ratio){continue;}
            //倾斜角度约束
            if(rect.angle>param.choose_floor_angle && rect.angle<param.choose_ceil_angle){continue;}
            lights.push_back(LightDescriptor(rect));
        }

        //进行匹配
        std::vector<MatchLights> matc;
        for(int i=0;i<int(lights.size()-1);i++)
        {
            for(int j=i+1;j<int(lights.size());j++)
            {
                if(abs(lights[i].angle-lights[j].angle)>param.match_dangle){continue;}
                if(abs(lights[i].center.x-lights[j].center.x)>param.match_max_dx){continue;}
                if(abs(lights[i].center.y-lights[j].center.y)>param.match_dy){continue;}
                if(abs(lights[i].length-lights[j].length)>param.match_dlength){continue;}
                matc.push_back(MatchLights(lights[i],lights[j]));
            }
        }

    //对匹配结果检查
        int i=0;
        while(i<int(matc.size()-1) && int(matc.size())!=0)
        {
            int j=i+1;
            while(j<int(matc.size()))
            {
                if(abs(matc[i].center.x-matc[j].center.x)<param.check_distance)
                {
                    if(matc[i].score>matc[j].score)
                    {matc.erase(matc.begin()+i);}
                    else{matc.erase(matc.begin()+j);}
                    continue;
                }
            j++;
            }
            i++;
        }
        
        //PnP解算
        Eigen::Vector3f xyz;
        std::ostringstream oss1,oss2,oss3;
        double fc_x,fc_y,cc_x,cc_y,coe1,coe2,coe3,coe4;
        
        //读取参数
        
        fc_x=this->get_parameter("fc_x").as_double();
        fc_y=this->get_parameter("fc_y").as_double();
        cc_x=this->get_parameter("cc_x").as_double();
        cc_y=this->get_parameter("cc_y").as_double();
        coe1=this->get_parameter("coe1").as_double();
        coe2=this->get_parameter("coe2").as_double();
        coe3=this->get_parameter("coe3").as_double();
        coe4=this->get_parameter("coe4").as_double();

        cv::Mat IntrinsicMatrix = (cv::Mat_<float>(3,3)<<cc_x,0,fc_x,0,cc_y,fc_y,0,0,1);
        cv::Mat coe=(cv::Mat_<float>(4,1)<<coe1,coe2,coe3,coe4);
        if(matc.size()!=0)
        {
            xyz=matc[0].PNP(IntrinsicMatrix,coe);
            oss1<<"x="<<xyz[0]<<"mm";
            oss2<<"y="<<xyz[1]<<"mm";
            oss3<<"z="<<xyz[2]<<"mm";
            
        }else{oss1<<"x=";oss2<<"y=";oss3<<"z=";}
        std::string str1(oss1.str());
        std::string str2(oss2.str());
        std::string str3(oss3.str());

        cv::putText(img,str1,cv::Point2i(20,50),cv::FONT_HERSHEY_COMPLEX,1,cv::Scalar(0,255,0),2);
        cv::putText(img,str2,cv::Point2i(20,90),cv::FONT_HERSHEY_COMPLEX,1,cv::Scalar(0,255,0),2);
        cv::putText(img,str3,cv::Point2i(20,130),cv::FONT_HERSHEY_COMPLEX,1,cv::Scalar(0,255,0),2);

    //显示画面
    for(int i=0;i<int(matc.size());i++){   
        matc[i].draw(img);
    }

    //std::cout<<str1<<std::endl<<str2<<std::endl<<str3<<std::endl;
    //std::cout<<"-----------"<<std::endl;
    cv::imshow("show",img);
    cv::waitKey(1);
    };
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SubNode>("subscribeb");
    node->declare_parameter("fc_x",0.0f);
    node->declare_parameter("fc_y",0.0f);
    node->declare_parameter("cc_x",0.0f);
    node->declare_parameter("cc_y",0.0f);
    node->declare_parameter("coe1",0.0f);
    node->declare_parameter("coe2",0.0f);
    node->declare_parameter("coe3",0.0f);
    node->declare_parameter("coe4",0.0f);
    rclcpp::spin(node);
    rclcpp::shutdown();
    cv::destroyAllWindows();
    return 0;
}
